// ----------------------------------------------------------------------------
//
//
// OpenSteer -- Steering Behaviors for Autonomous Characters
//
// Copyright (c) 2002-2005, Sony Computer Entertainment America
// Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
//
// ----------------------------------------------------------------------------
//
// Vec3: OpenSteer's generic type for 3d vectors
//
// This file defines the class Vec3, which is used throughout OpenSteer to
// manipulate 3d geometric data.  It includes standard vector operations (like
// vector addition, subtraction, scale, dot, cross...) and more idiosyncratic
// utility functions.
//
// When integrating OpenSteer into a preexisting 3d application, it may be
// important to use the 3d vector type of that application.  In that case Vec3
// can be changed to inherit from the preexisting application' vector type and
// to match the interface used by OpenSteer to the interface provided by the
// preexisting 3d vector type.
//
// 10-04-04 bk:  put everything into the OpenSteer namespace
// 03-26-03 cwr: created to replace for Hiranabe-san's execellent but larger
//               vecmath package (http://objectclub.esm.co.jp/vecmath/)
//
// ----------------------------------------------------------------------------


#ifndef OPENSTEER_VEC3_H
#define OPENSTEER_VEC3_H

#include <vector>    // for std::vector
#include <ostream>
#include <math.h>

//#include "OpenSteer/Utilities.h"  // for interpolate, etc.

class Vec3
{
public:

    // ----------------------------------------- generic 3d vector operations

    // three-dimensional Cartesian coordinates
    float x, y, z;

    // constructors
    Vec3 (void): x( 0.0f ), y( 0.0f ), z( 0.0f ) {}
    Vec3 (float X, float Y, float Z) : x( X ), y( Y ), z( Z ) {}

    // vector addition
    Vec3 operator+ (const Vec3& v) const {return Vec3 (x+v.x, y+v.y, z+v.z);}

    // vector subtraction
    Vec3 operator- (const Vec3& v) const {return Vec3 (x-v.x, y-v.y, z-v.z);}

    // unary minus
    Vec3 operator- (void) const {return Vec3 (-x, -y, -z);}

    // vector times scalar product (scale length of vector times argument)
    Vec3 operator* (const float s) const {return Vec3 (x * s, y * s, z * s);}

    // vector divided by a scalar (divide length of vector by argument)
    Vec3 operator/ (const float s) const {return Vec3 (x / s, y / s, z / s);}

    // dot product
    float dot (const Vec3& v) const {return (x * v.x) + (y * v.y) + (z * v.z);}

    // length
    float length (void) const {return sqrt (lengthSquared ());}

    // length squared
    float lengthSquared (void) const {return this->dot (*this);}

    // normalize: returns normalized version (parallel to this, length = 1)
    Vec3 normalize (void) const
    {
        // skip divide if length is zero
        const float len = length ();
        return (len>0) ? (*this)/len : (*this);
    }

    // cross product (modify "*this" to be A x B)
    // [XXX  side effecting -- deprecate this function?  XXX]
    void cross(const Vec3& a, const Vec3& b)
    {
        *this = Vec3 ((a.y * b.z) - (a.z * b.y),
                      (a.z * b.x) - (a.x * b.z),
                      (a.x * b.y) - (a.y * b.x));
    }

    // assignment
    Vec3 operator= (const Vec3& v) {x=v.x; y=v.y; z=v.z; return *this;}

    // set XYZ coordinates to given three floats
    Vec3 set (const float _x, const float _y, const float _z)
    {x = _x; y = _y; z = _z; return *this;}

    // +=
    Vec3 operator+= (const Vec3& v) {return *this = (*this + v);}

    // -=
    Vec3 operator-= (const Vec3& v) {return *this = (*this - v);}

    // *=
    Vec3 operator*= (const float& s) {return *this = (*this * s);}

    
    Vec3 operator/=( float d ) { return *this = (*this / d);  }
    
    // equality/inequality
    bool operator== (const Vec3& v) const {return x==v.x && y==v.y && z==v.z;}
    bool operator!= (const Vec3& v) const {return !(*this == v);}

    // @todo Remove - use @c distance from the Vec3Utilitites header instead.
    // XXX experimental (4-1-03 cwr): is this the right approach?  defining
    // XXX "Vec3 distance (vec3, Vec3)" collided with STL's distance template.
    static float distance (const Vec3& a, const Vec3& b){ return(a-b).length();}

    // --------------------------- utility member functions used in OpenSteer

    // return component of vector parallel to a unit basis vector
    // (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))

    inline Vec3 parallelComponent (const Vec3& unitBasis) const
    {
        const float projection = this->dot (unitBasis);
        return unitBasis * projection;
    }

    // return component of vector perpendicular to a unit basis vector
    // (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))

    inline Vec3 perpendicularComponent (const Vec3& unitBasis) const
    {
        return (*this) - parallelComponent (unitBasis);
    }

	inline Vec3 reverse() {
		return (*this * -1.0f);
	}

    // clamps the length of a given vector to maxLength.  If the vector is
    // shorter its value is returned unaltered, if the vector is longer
    // the value returned has length of maxLength and is paralle to the
    // original input.

    Vec3 truncateLength (const float maxLength) const
    {
        const float maxLengthSquared = maxLength * maxLength;
        const float vecLengthSquared = this->lengthSquared ();
        if (vecLengthSquared <= maxLengthSquared)
            return *this;
        else
            return (*this) * (maxLength / sqrt (vecLengthSquared));
    }

    // forces a 3d position onto the XZ (aka y=0) plane

    Vec3 setYtoZero (void) const {return Vec3 (this->x, 0, this->z);}

    // rotate this vector about the global Y (up) axis by the given angle

    Vec3 rotateAboutGlobalY (float angle) const 
    {
        const float s = sin (angle);
        const float c = cos (angle);
        return Vec3 ((this->x * c) + (this->z * s),
                     (this->y),
                     (this->z * c) - (this->x * s));
    }

    // version for caching sin/cos computation
    Vec3 rotateAboutGlobalY (float angle, float& theSin, float& theCos) const 
    {
        // is both are zero, they have not be initialized yet
        if (theSin==0 && theCos==0)
        {
            theSin = sin (angle);
            theCos = cos (angle);
        }
        return Vec3 ((this->x * theCos) + (this->z * theSin),
                     (this->y),
                     (this->z * theCos) - (this->x * theSin));
    }

    // if this position is outside sphere, push it back in by one diameter

    Vec3 sphericalWrapAround (const Vec3& center, float radius)
    {
        const Vec3 offset = *this - center;
        const float r = offset.length();
        if (r > radius)
            return *this + ((offset/r) * radius * -2);
        else
            return *this;
	}

	//retorna um angulo entre -PI e PI
	float angle(const Vec3& v)
	{
		float dp, angPI ;

		dp = dot( v ); //dot product
		if(dp >= 1.0) dp = 1.0f;
		if(dp <=-1.0) dp =-1.0f;

		angPI = (float)acos( dp );

		//determina a posicao relativa do vetor
		return angPI;
	}

	//todo: fazer isso ;)
	//int side(Vec3 v) {
	//	return 1;
	//}

	void print(void)
	{
		printf("\n %f %f %f", x, y, z);
	}

	static Vec3 RandomUnitVector() {
		Vec3 result((float)rand(), (float)rand(), (float)rand());
		result = result.normalize();
		return result;
	}

    // names for frequently used vector constants
    static const Vec3 zero;
    static const Vec3 side;
    static const Vec3 up;
    static const Vec3 forward;
};


// ----------------------------------------------------------------------------
// scalar times vector product ("float * Vec3")


inline Vec3 operator* (float s, const Vec3& v) {return v*s;}


// return cross product a x b
inline Vec3 crossProduct(const Vec3& a, const Vec3& b)
{
	Vec3 result((a.y * b.z) - (a.z * b.y),
				(a.z * b.x) - (a.x * b.z),
				(a.x * b.y) - (a.y * b.x));
	return result;
}

// ----------------------------------------------------------------------------
// default character stream output method

#ifndef NOT_OPENSTEERDEMO  // only when building OpenSteerDemo

inline std::ostream& operator<< (std::ostream& o, const Vec3& v)
{
    return o << "(" << v.x << "," << v.y << "," << v.z << ")";
}

#endif // NOT_OPENSTEERDEMO

// ----------------------------------------------------------------------------
// Returns a position randomly distributed inside a sphere of unit radius
// centered at the origin.  Orientation will be random and length will range
// between 0 and 1


Vec3 RandomVectorInUnitRadiusSphere (void);


// ----------------------------------------------------------------------------
// Returns a position randomly distributed on a disk of unit radius
// on the XZ (Y=0) plane, centered at the origin.  Orientation will be
// random and length will range between 0 and 1


Vec3 randomVectorOnUnitRadiusXZDisk (void);

// ----------------------------------------------------------------------------
// Returns a position randomly distributed on a circle of unit radius
// on the XZ (Y=0) plane, centered at the origin.  Orientation will be
// random and length will be 1


inline Vec3 RandomUnitVectorOnXZPlane (void)
{
    return RandomVectorInUnitRadiusSphere().setYtoZero().normalize();
}


// ----------------------------------------------------------------------------
// used by limitMaxDeviationAngle / limitMinDeviationAngle below


Vec3 vecLimitDeviationAngleUtility (const bool insideOrOutside,
                                    const Vec3& source,
                                    const float cosineOfConeAngle,
                                    const Vec3& basis);


// ----------------------------------------------------------------------------
// Enforce an upper bound on the angle by which a given arbitrary vector
// diviates from a given reference direction (specified by a unit basis
// vector).  The effect is to clip the "source" vector to be inside a cone
// defined by the basis and an angle.


inline Vec3 limitMaxDeviationAngle (const Vec3& source,
                                    const float cosineOfConeAngle,
                                    const Vec3& basis)
{
    return vecLimitDeviationAngleUtility (true, // force source INSIDE cone
                                          source,
                                          cosineOfConeAngle,
                                          basis);
}


// ----------------------------------------------------------------------------
// Enforce a lower bound on the angle by which a given arbitrary vector
// diviates from a given reference direction (specified by a unit basis
// vector).  The effect is to clip the "source" vector to be outside a cone
// defined by the basis and an angle.


inline Vec3 limitMinDeviationAngle (const Vec3& source,
                                    const float cosineOfConeAngle,
                                    const Vec3& basis)
{    
    return vecLimitDeviationAngleUtility (false, // force source OUTSIDE cone
                                          source,
                                          cosineOfConeAngle,
                                          basis);
}


// ----------------------------------------------------------------------------
// Returns the distance between a point and a line.  The line is defined in
// terms of a point on the line ("lineOrigin") and a UNIT vector parallel to
// the line ("lineUnitTangent")


inline float distanceFromLine (const Vec3& point,
                               const Vec3& lineOrigin,
                               const Vec3& lineUnitTangent)
{
    const Vec3 offset = point - lineOrigin;
    const Vec3 perp = offset.perpendicularComponent (lineUnitTangent);
	return perp.length();
}


// ----------------------------------------------------------------------------
// given a vector, return a vector perpendicular to it (note that this
// arbitrarily selects one of the infinitude of perpendicular vectors)


Vec3 findPerpendicularIn3d (const Vec3& direction);


// ----------------------------------------------------------------------------
// candidates for global utility functions
//
// dot
// cross
// length
// distance
// normalized

    
// ----------------------------------------------------------------------------
#endif // OPENSTEER_VEC3_H
